#!/usr/bin/env python

import roslib; roslib.load_manifest('roscolbot')
import rospy
import math
import threading

from sensor_msgs.msg import JointState
from rosqwerk.msg import ServoStates
from rosqwerk.msg import MotorStates

# position of motor when neck is full up
MAX_MOTOR_POS_NECK=-84000.0

class FakeJointPublisherTrigger(threading.Thread):
    def __init__(self, jointStatesPublisher, publishFrequency):       
        threading.Thread.__init__(self)        
        self.threadName = "Fake Joint State Publisher"        
        self.publishRate = rospy.Rate(publishFrequency)    
        self.jointStatesPublisher=jointStatesPublisher  
        
    def run(self):
        rospy.loginfo("Fake joint publisher starting")        
        while not rospy.is_shutdown():                
            self.jointStatesPublisher.publishJoints()      
            self.publishRate.sleep()    

class JointStatesPublisher():
    """
    Listen for servos and motors from Qwerk and publish joint states accordingly
    """
    def __init__(self):
        rospy.init_node('colbot_joint_states_publisher', anonymous=True)
                        
        self.jointStatesPublisher = rospy.Publisher('joint_states', JointState)        
        rospy.Subscriber('qwerk/servo_states',ServoStates, self.readServos)
        rospy.Subscriber('qwerk/motor_states', MotorStates, self.readMotors)
        
        # -40 deg..+40 deg (rad)
        self.head_pan_joint_value=0
        
        # -40 deg..+40 deg (rad)
        self.head_tilt_joint_value=0
        
        # 0..0.36 (meters)
        self.neck_joint_value=0    
        
        # do we want to publish fake joints (useful if qwerk is not available,
        # to publish tf transforms anyway):
        publish_fake_joints = rospy.get_param('~publish_fake_joints', 'false') 
        publish_fake_joints_frequency = rospy.get_param('~publish_fake_joints_frequency', 50)  
        if publish_fake_joints:
            rospy.loginfo("Publishing fake joints at frequency %d" % publish_fake_joints_frequency)   
            FakeJointPublisherTrigger(self,publish_fake_joints_frequency).start()    
                                                                                                                     
    def publishJoints(self):
        msg = JointState()
        msg.name = list()
        msg.position = list()
        msg.velocity = list()
        msg.effort = list()
        
        msg.name.append('neck_joint')
        msg.position.append(self.neck_joint_value)

        msg.name.append('head_pan_joint')
        msg.position.append(self.head_pan_joint_value)

        msg.name.append('head_tilt_joint')
        msg.position.append(self.head_tilt_joint_value)
                
        msg.header.stamp = rospy.Time.now()
        self.jointStatesPublisher.publish(msg)

    def readServos(self,servoStates):
        self.head_pan_joint_value=math.radians(((servoStates.servoPositions[0]-127)/(80.0/255)))
        self.head_tilt_joint_value=math.radians(((servoStates.servoPositions[1]-127)/(80.0/255)))
        self.publishJoints()

    def readMotors(self,motorStates):
        self.neck_joint_value=abs(0.36*(motorStates.motorPositions[0]/MAX_MOTOR_POS_NECK))
        
        
if __name__ == '__main__':
    try:
        JointStatesPublisher()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass

